/***************************************************************************** 
** board/infotm/imapx/lowlevel_init.S
** 
** Copyright (c) 2009~2014 ShangHai Infotm Ltd all rights reserved. 
** 
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation; either version 2 of the License, or
** (at your option) any later version.
** 
** Description: Board specific low level initilizations.
**
** Author:
**     Warits   <warits.wang@infotm.com>
**      
** Revision History: 
** ----------------- 
** 1.1  XXX 12/24/2009 XXX	Warits
*****************************************************************************/

/*
 * TODO: U-BOOT use only SVC32 mode, so FIQ, IRQ stack is not set
 *		 be different with FPGA init code, INT is not disable after
 *		 reset.
 *	fix these if any problem happens.
 */

#include <config.h>
#include <version.h>

#include <asm/arch/imapx200.h>

.globl lowlevel_init
lowlevel_init:
	mov r5, lr		@ save return address

	/*
	 * remap peripheral port to pre-defined PERIPHERAL_BASE_ADDR_PA(0x20c00000)
	 * size is 0x13 (256MB)
	 */
	ldr r0, =PERIPHERAL_BASE_ADDR_PA
	orr r0, r0, #0x13
	mcr p15, 0, r0, c15, c2, 4
	
	/* mem swap onto SRAM */
	mov r1, #0x01
	str r1, [r0, #0x108]

sys_power:
	/*	power relative instructions may add here, e.g. PULL UP GPN14 */

	/* AHB peripheral bus output enable */
	ldr r0, =PERIPHERAL_BASE_ADDR_PA
	ldr r1, =0xffffffff
	str r1, [r0, #0x228]

	/* power on modules (iDSP, iGPU, venc, vdec, iGPS, mempoll) */
	ldr r1, =0x3f
	str r1, [r0, #0x210]
	/* FIXME: 0x214 should be read only */
	str r1, [r0, #0x218]

misc_init:
	/* add disable watchdog, if needed */
	ldr r2, =WDT_BASE_REG_PA
	mov r1, #0
	str r1, [r2]

	/* mask out all interrupts */
	orr r2, r0, #0xb0000			/* INTC 0x20cb0000 */
	ldr r1, =0xffffffff
	str r1, [r2, #0x08]			/* INTMSK1 */
	str r1, [r2, #0x2c]			/* INTMSK2 */

	/* disable xxx acc */
	orr r2, r0, #0x5000
#ifdef CONFIG_IMAPX200_INSP
	mov r1, #CONFIG_IMAPX200_INSP
#else
	/* Turn on InsP as default */
	mov r1, #0x46
#endif
	str r1, [r2, #4]

	/* disable debug settings */
	ldr r1, =0xff
	str r1, [r0, #0x204]

#ifdef CONFIG_SYS_BOOT_NOR
nor_manage:
	/* if run in nor, copy to BRAM */
	ldr r2, =0x20c00108
	ldr r1, [r2]
	tst r1, #1
	bne wakeup_reset

	mov r0, #0
	ldr r1, =0x3e000000
	mov r3, #0
nor_bram_l:
	ldr r2, [r0, r3]
	str r2, [r1, r3]
	add r3, r3, #4
	cmp r3, #0x2000
	bne nor_bram_l
	ldr r2, =0x20c00108
	mov r1, #1
	str r1, [r2]
	mov pc, #0
#endif

wakeup_reset:
	/* bit 14~31 marked as reserved on datasheet */
	ldr	r2, [r0, #0x204]
	ldr r1, =0x3ff
	bic r2, r2, r1, lsl #16
	str r2, [r0, #0x204]

sys_clk_init:
 	ldr r0,=DIV_CFG1
	ldr r1,=0x16160fde
	str r1,[r0]

	ldr r0,=DIV_CFG2
	ldr r1,=0x16260102
	str r1,[r0]

	ldr r0,=DIV_CFG3
	ldr r1,=0x030303
	str r1,[r0]

	ldr r0,=DIV_CFG4
	ldr r1,=0x160e
	str r1,[r0]

	ldr r0,=PLL_CLKSEL
	ldr r1,=0x0
	str r1,[r0]

	ldr r0,=DPLL_CFG
	ldr r1,=0x80001027
	str r1,[r0]

	ldr r0,=EPLL_CFG
	ldr r1,=0x80001023
	str r1,[r0]

#if defined(CONFIG_CLK_MANUAL_MODE)
	/* get config parameter */
	mov r0, #0
	ldr r1,=GPECON
	str r0,[r1]
	ldr r1,=GPEDAT
	ldr r0,[r1]
	lsr r0, r0, #1
	ldr r1, =0xffffffe7
	bic r2, r0, r1		@r2 is manual freq offset

	b apll_cfg

_manual_freq_v:
	.word	.
	.word	CONFIG_SYS_APLL_MV1
	.word	CONFIG_SYS_DCFG_MV1
	.word	CONFIG_SYS_APLL_MV2
	.word	CONFIG_SYS_DCFG_MV2
	.word	CONFIG_SYS_APLL_MV3
	.word	CONFIG_SYS_DCFG_MV3
	.word	CONFIG_SYS_APLL_MV4
	.word	CONFIG_SYS_DCFG_MV4

apll_cfg:
	ldr r1,=APLL_CFG
	ldr r3,_manual_freq_v
	add r3,r3,#4
	add r3,r3,r2
	ldr r0,[r3]			@manual apll value
	str r0,[r1]

	ldr r1,=DIV_CFG0
	ldr r0,[r3, #4]
	str r0,[r1]

#else
	/* CLK autoconfig remove by warits */
#if 0
	ldr r1, =CONFIG_AU_CLK_MAGIC_OFFS
	ldr r0, [r1]						@r0: CLK_MAGIC
	ldr r2, =CONFIG_AU_CLK_MAGIC
	cmp r0, r2

	/* goto normal set up if adjust number is not valid */
	bne apll_cfg

	ldr r1, [r0, #4]		@get apll config
	ldr r2, [r0, #8]		@get dcfg config

	b apll_cfg2
apll_cfg:
#endif

	ldr r1,=CONFIG_SYS_IMAPX200_APLL
	ldr r2,=CONFIG_SYS_IMAPX200_DCFG0

apll_cfg2:
	ldr r0,=APLL_CFG
	str r1,[r0]
	ldr r0,=DIV_CFG0
	str r2,[r0]
#endif

pll_lock_loop:
	ldr r0,=PLL_LOCKED
	ldr r1,[r0]
	ldr r2,=0xfffffff8
	bic r3,r1,r2
	cmp r3,#7
	bne pll_lock_loop

	ldr r0,=PLL_OCLKSEL
	ldr r1,=0x7	@ swith all clock to PLL out clk
	str r1,[r0]

	ldr r0,=CPUSYNC_CFG
@	ldr r1,=CONFIG_SYS_CPUSYNC	@Set CPU sync mode
	mov r1,#0					@Set CPU sync mode
	str r1,[r0]
	
	ldr r0,=NPOW_CFG
@	ldr r1,=0xef	@power on all module except GPU
	ldr r1,=0x3f	@power on off module
	str r1,[r0]

	bl mem_ctrl_asm_init

	mov lr, r5			@ restore return address
	mov pc, lr
